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Uhhh... Inverse Kinematics? 
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Problem Description 

• We have a bunch of rigid bodies aka links 
(aka bones). 

• Pairs of links are connected by joints. 

• A joint limits the degrees of freedom 
(DoFs) of one link relative to the other. 

• Connection graph is a tree. No loops! 
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Problem Description (cont'd) 

• Let's consider 1-DoF joints only: 

• Revolute : single-axis rotation aka hinge. 

• Prismatic : single-axis translation aka slider. 

• Positions and velocities of links are 
defined by the values and speeds of the 
scalar joint parameters (angles, 
distances). 
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Problem Description (cont'd) 



Revolute 

Joint 


GAME DEVELOPERS CONFERENCE® 2015 


MARCH 2-6, 2015 GDCONF.COM 


Problem Description (cont'd) 

• Given some constraints on the poses and 
velocities of one or more links, compute a 
vector of joint parameters that satisfies 
the constraints. 

• The constrained links are called end- 
effectors , and are usually (but not per se) 
the end-links of a linkage. 
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Free vs. Fixed Joints 

• Usually, only a few joints are free. Free 
joints are available for constraint 
resolution. 

• The other joints are controlled by forward 
kinematics. Their positions and velocities 
are fixed at a given instance of time. 
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Part I: Angular Constraints 
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Rotations in 3D 

• Have three degrees of freedom (DoFs). 

• Do not commute: R X R 2 * R 2 Ri 

• Can be parameterized by three angles 
about predefined axes (Euler angles). 

• Angle parameterization is not ideal for 
doing math (gimbal lock). 
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Quaternions 

• Quaternions extend complex numbers 
q = a + bi + cj + dk 

where a, b, c and d are real numbers 

• a is the real or scalar part, and 

• (b, c, d) is the imaginary or vector part. 
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Quaternions (cont'd) 

• Quaternions behave as 4D vectors w.r.t. 
addition and scaling. 

• In multiplications, the imaginary units 
resolve as: i 2 = j 2 = k 2 = ijk = -1 

• In scalar-vector notation, multiplication is 
given by: [s u vj[s 2 , v 2 ] = 

[ 5^2 " Vj • V 2 SiV 2 + S 2 V! +VjX V 2 ] 
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Quaternion Conjugate 

• The conjugate of quaternion q, denoted 
by q*, is defined by 

(a + bi + cj + dk)* = a - bi - cj - dk 

• Multiplication of a quaternion by its 
conjugate yields its squared magnitude: 
qq* = q*q = a 2 + b 2 + c 2 + d 2 
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Unit Quaternions 

• Unit quaternions (points on sphere in 4D) 
form a multiplicative subgroup. 

• A rotation with angle 6 about unit vector 
u is represented by unit quaternion 
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Rotations using Unit Quaternions 

• The so-called sandwich product performs 
a rotation: v' = q v q* 

• The vector v is regarded as a pure 
imaginary quaternion. 

• The conjugate is the inverse rotation: 

v = q* v' q 



GAME DEVELOPERS CONFERENCE® 2015 


MARCH 2-6, 2015 GDCONF.COM 


Kinematic Chain 

• In a chain of links, r, is the relative 
rotation from link / to its parent link / - 1. 

• The rotation from a link / to the world 
frame is simply q, = i^ — r jt the product of 
relative rotations in the chain up to link /. 

• The rotation from link / to link j is: q ; *q, 
(even if / and j are on different chains). 
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There's a Twist... 

• Unit quaternions q and -q represent the 
same orientation. 

• For computing the rotation q 7 *q, from q, 
to q jf make sure that q,and q 7 point in the 
same direction (q, * q 7 > 0), if necessary, 
by negating either q, or q \ Jm 

• Otherwise, q 7 *q, takes an extra spin. 
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Angular Velocity 

• The angular velocity of a rigid body is a 

3D vector. / / 

• Its direction points along the rotation axis 
following the right-hand rule/ A O 

• Its magnitude is the rotational speed in 
radians per second. 
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Angular Velocity 

• Angular velocity is 
a proper vector: 

• The angular 
velocity of a link is 
the sum of all joint 
velocities along the 
chain. 
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X 



GAME DEVELOPERS CONFERENCE® 2015 


MARCH 2-6, 2015 GDCONF.COM 


Angular Velocity Demo 
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Joint Velocity 

• The directions of the joint axes form a 
vector space for the angular velocity <o of 
an end-effector: 

• • 

<0 = a 1 0 1 + — 1- a n 0 n 

• Here, 0 t are the joint speeds in radians 
per second. 
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Joint Velocity (cont'd) 

• In matrix notation this looks like 



• The matrix columns are the n joint axes. 
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Joint Axis Direction 

• Given q„ link i's rotation relative to the 
world frame, the direction of the joint 
axis is the local rotation axis u, in world 
coordinates: 

a; = qiU;q;* 
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Velocity Constraints 

• A velocity constraint is defined by a linear 
function that maps velocities to vectors. 

• The dimension of the resulting vector is 
the number of constrained DoFs. 

• The constraint is satisfied if the function 
returns the target value (usually zero). 



GAME DEVELOPERS CONFERENCE® 2015 


MARCH 2-6, 2015 GDCONF.COM 


Rotational Axis Constraint 

• Constrains the axis of rotation of an end- 
effector link to some target axis. 

• For example, for constraint function 
C(co) = (o) x , a> y ), imposing C(a>) = 0 


restricts the axis to the Z-axis. 
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Constraint Matrix 

• A constraint involving a linear function C 
and target t can be expressed as 


CM = CM 
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Free & Fixed Joint Parameters 

• Move the fixed joint parameters over to 
the right-hand side 

f c (a i+1 ) ••• C(a n )|( i ) = t - (Cfai)^ -I l-C(a,)0,) 


Here, only 6 i+1 to 6 n are variables. 
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Jacobian Matrix 

• The remaining matrix expresses the 
influence of variable joint speeds on the 
constraint function. 

• This is in fact the Jacobian matrix. 

• #rows = #constrained DoFs. 

• #colums = #free joint parameters. 
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No Inverse 

• The Jacobian matrix generally does not 
have an inverse. 

• Often the matrix is not square, and thus 
not invertible. 

• Square Jacobians may not be invertible, 
since they can have dependent columns. 
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Too Few Variables 

• The constraints fix more DoFs than there 
are variables: 

/ = (c(a n _i) C( a n ) 

> • • 



Likely, no solution exists. We settle for a 
best-fit solution. 
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Too Many Variables 

• The constraints fix fewer DoFs than there 
are variables: 

/ = K(a n _ 3 ) - C( a n ) 



Infinitely many solutions may exist. We 
seek the lowest speed solution. 
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Pseudoinverse 


• The Moore-Penrose pseudoinverse J + is 
(j T j) V T if #rows > #colums 

7 T (77 T ) 1 if #rows < #colums 


Giving 



J + (t - (C (a^ 
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Pseudoinverse (cont'd) 

• If no solution exists, returns a best-fit 
(least-squares) solution. 

• If infinitely many solutions exist, returns 
the least-norm (lowest speed) solution. 

• If an inverse exists, the pseudoinverse is 
the inverse. 



GAME DEVELOPERS CONFERENCE® 2015 


MARCH 2-6, 2015 GDCONF.COM 


Computing the Pseudoinverse 

• y + can be computed using open-source 
linear-algebra packages (Eigen, 
Armadillo+LAPACK). 

• Cubic complexity! ( 0(n 3 ) for n variables) 

• Decimate into smaller Jacobians, rather 
than solve one huge Jacobian. 
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Gimbal Lock Demo 
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Positional Error 

• Constraint solving happens at sampled 
intervals. 

• Jacobian is falsely assumed to be fixed 
in-between samples. 

• Positional error builds up (drift). 
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Positional Error (cont'd) 

• Correct error by adding a stabilization 
term to the target vector: 

Corrects 

error 


J + (t - (C(a 1 )0 1 H 1- C(ai)0i) + s) 
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Positional Error (cont'd) 

• We choose s = C(to diff ), where <o diff is the 
angular velocity that closes the gap 
between q n and q t , 
the orientations of 
resp. end -effector 
and target. 
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Positional Error (cont'd) 

• A useful approximation for « diff is the 
vector part of 

~ 2 _ _ * 

• Here, h is the time interval. 

• Factor /? (< 1) relaxes correction speed. 

• N.B.: Mind the extra spin when q n • q t < 0! 
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Part II: Rigid-Body Constraints 
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Chasles' Theorem 

• A screw is a rotation about a 
line and a translation along 
the same line. 

• "Any rigid-body displacement 
can be defined by a screw." 
(Michel Chasles, 1830) 
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Demo 
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Screw Theory 


• "By replacing vectors 
(directions) with Plucker 
coordinates (lines), point 
entities (angular velocity , 
force ) transfer to rig id -body 
entities (twist, wrench)." 

(Sir Robert Stawell Ball, 1876) 
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Dual Quaternions 

• Quaternion algebra is extended by 
introducing a dual unit s. 

• Elements are 1, i, , k, s, is, js, and ks. 

• A dual quaternion is expressed as: 

q = q + q's 

We call q the real part and q' the dual 
part. 
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Dual Quaternions (cont'd) 

• In multiplications, the dual unit resolves 
as e 2 =0, giving: (q x + q[£)(q 2 + q 2 £) 

= qiqz + (qiq'z + qM^ + o 


Real part is the product of real parts 
only; it does not depend on dual parts! 
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Dual Quaternions (cont'd) 

• The conjugate of a dual quaternion: 

q* = (q + qV>*= q* + q'*£ 


Multiplication of a dual quaternion by its 
conjugate yields its squared magnitude: 
(q + q's)(q + qY)‘ = qq* + (qq'* + q'q*> 
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Dual Quaternions (cont'd) 

• Unit dual quaternions (1 + Oe) represent 
rigid body displacements aka poses. 

• The rigid body pose given by unit (real) 
quaternion q and translation vector t is: 




t is considered a pure 
imaginary quaternion 
(zero scalar part). 
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Where is the Screw? 

• A unit dual quaternion can be written as 

(0 + ds\ [O + ds 

C0S [—2 ~ j ' Sin [~^ 2 ~ 

d is the rotation angle, 
d\s the translation distance, and 
u + v£ is the screw axis as unit dual vector 
(Plucker coordinates). 


^ (U + Vf) 
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Linear Velocity 

• Linear velocity, unlike angular velocity, is 
bound to a point in space: 



I 
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Linear Velocity (cont'd) 

• Given angular velocity a>, and linear 
velocity v at point p, the linear velocity at 
an arbitrary point x is v + x (x - p). 



x 
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Plucker Coordinates 

• Angular and linear velocity are combined 
into a single entity represented by a dual 
vector (aka Plucker coordinates): 

V — CO + V°£ 

• Here, v° is the linear velocity at the origin 
of the coordinate frame. 
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Plucker Coordinates Demo 

D C?B 
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Transforming Plucker Coordinates 

• The dual-quaternion sandwich product 
performs a rigid-body transformation on 
Plucker coordinates: 

f ^ A 

v - q v q 

• This transformation preserves 
magnitude: v' • v' = v • v 
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Deja Vu? 

• The (combined) velocity of a link is the 
sum of all joint velocities along the chain. 

• The joint axes form a vector space for 
the velocity v of an end-effector: 

V = aA + ••• + a n 0 n 

• Here, are the revolute and prismatic 
joint speeds. 
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Deja Vu? (cont'd) 


• For q ir link i's pose expressed in the 
world frame, tij, the local joint axis, the 
joint axis in world coordinates is 

A /A /A /A 

a* = q;U;q; 


• For a revolute: 
u; = u, + 0£ 


For a prismatic: 

U; = 0 + V;£ 
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Deja Vu? (cont'd) 

• To correct the positional error between 
end-effector and target, we choose the 
correction velocity v diff to be the vector 
part of 

2 ^ ^ * 

%q t qn 
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The Principle of Transference 


Angular Entities 

Rigid-body Entities 

Rotation 

unit quaternion 

Pose (screw) 

unit dual quaternion 

Angular 

velocity 

3-vector 

Combined 

velocity 

dual 3-vector 

Direction 

unit 3-vector 

Line 

unit dual 3-vector 

Rotation 

parameter 

angle (radians) 

Screw 

parameters 

dual angle (radians, 
meter) 

Spherical 
coordinates 
(azi, polar) 

pair of angles 

Denavit- 

Hartenberg 

parameters 

pair of dual angles 
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Open-Source Code 

• Eigen: A C++ Linear Algebra Library. 
http://eiqen.tuxfamilv.org . License: MPL2 

• Armadillo: C++ Linear Algebra Library. 
http://arma.sourceforqe.net . License: MPL2 

• LAPACK - Linear Algebra PACKage. 
http://www.netlib.org/lapack . License: BSD 

• MoTo C++ template library (dual quaternion code) 
https://code.qooqle.eom/p/motion-toolkit/ . License: MIT 
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Thank You! 

My pursuits can be traced on: 

• Web: http://www.dtecta.com 

• Twitter: @dtecta 

• Or just mail me: qino@dtecta.com 



